# 从大文件逐行读取并解析数据

import numpy as np

def lodar_lidar_data(path):

    # 打开并读取文本文件
    with open(path, 'r') as file:
        lines = file.readlines()

    # 初始化一个列表来存储字典
    data_list = []

    # 初始化一个临时字典来存储当前的块
    current_dict = {}

    for line in lines:
        line = line.strip()  # 去除行末的换行符和空格
        
        if line.startswith('time_stamp:'):
            # 如果遇到新的 time_stamp，且当前字典非空，则将其添加到列表中
            if current_dict:
                data_list.append(current_dict)
            
            # 创建新的字典并存储 time_stamp
            current_dict = {'time_stamp': line.split(': ')[1]}
        
        elif line.startswith('fusepose:'):
            # 存储 fusepose 数据
            current_dict['fusepose'] = line.split(': ')[1]
        
        elif line.startswith('rpy:'):
            # 存储 rpy 数据
            current_dict['rpy'] = line.split(': ')[1]

    # 添加最后一个字典到列表中（如果存在）
    if current_dict:
        data_list.append(current_dict)

    # 输出结果
    # for item in data_list:
    #     print(item)

    print(f'lidar data len {data_list.__len__()}')
    return data_list


def load_ai_data(data_path):
    # 打开并读取文本文件
    with open(data_path, 'r') as file:
        lines = file.readlines()

    # 初始化一个列表来存储数据帧
    frames = []

    # 初始化一个临时字典来存储当前的帧
    current_frame = {}

    for line in lines:
        line = line.strip()  # 去除行末的换行符和空格
        
        if line.startswith('time_stamp:'):
            # 如果遇到新的 time_stamp，且当前帧非空，则将其添加到列表中
            if current_frame:
                frames.append(current_frame)
            
            # 创建新的帧并存储 time_stamp
            current_frame = {'time_stamp': line.split(': ')[1], 'xyz': []}
        
        elif line.startswith('xyz:'):
            # 存储 xyz 数据，过滤重复项
            xyz_value = line.split(': ')[1]
            if xyz_value not in current_frame['xyz']:
                current_frame['xyz'].append(xyz_value)

    # 添加最后一个帧到列表中（如果存在）
    if current_frame:
        frames.append(current_frame)

    # 输出结果
    # for frame in frames:
    #     print(frame)
    print(f'frames len {frames.__len__()}')
    return  frames

def load_lidar_ai_data(li_path, ai_path):
    ai_data = load_ai_data(ai_path)
    li_data = lodar_lidar_data(li_path)


    return ai_data, li_data


def matching_pose_with_ai(ai_data, li_data):
    pass
    result = []
    i, j = 0, 0
    threshold = 50
    while i < len(li_data) and j < len(ai_data):
        # 计算时间戳差
        diff = int(li_data[i]["time_stamp"]) - int(ai_data[j]["time_stamp"])
    
        if abs(diff) < threshold:
            tmp  = li_data[i]
            tmp['xyz'] = ai_data[j]['xyz']
            tmp['time_stamp1'] = ai_data[j]['time_stamp']
            tmp['diff'] = diff
            result.append(tmp)
            j += 1  # 移动 B 的指针
        elif diff < 0:
            i += 1  # A[i] < B[j]，移动 A 的指针
        else:
            j += 1  # A[i] > B[j]，移动 B 的指针
    
    return result


if __name__ == '__main__':

    li_path = '/home/JSDC/017254/code/gitee/map_learing/map_grid/04_xyzrpy_visual/rpy/data4/li.txt'
    ai_path = '/home/JSDC/017254/code/gitee/map_learing/map_grid/04_xyzrpy_visual/rpy/data4/ai.txt'

    ai_data, li_data = load_lidar_ai_data(li_path, ai_path)
    pass
    res = matching_pose_with_ai(ai_data, li_data)
    i = 1

# def find_close_timestamps(A, B, threshold=50):
#     result = []
#     i, j = 0, 0
#     while i < len(A) and j < len(B):
#         # 计算时间戳差
#         diff = A[i] - B[j]
        
#         if abs(diff) < threshold:
#             result.append(B[j])
#             j += 1  # 移动 B 的指针
#         elif diff < 0:
#             i += 1  # A[i] < B[j]，移动 A 的指针
#         else:
#             j += 1  # A[i] > B[j]，移动 B 的指针
    
#     return result

# # 示例
# A = [1000, 1050, 1100, 1150, 1200]
# B = [1020, 1070, 1120, 1160, 1210]

# close_timestamps = find_close_timestamps(A, B)
# print(close_timestamps)
